#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import time
import logging
import threading
from serial_handler import SerialHandler
from socket_handler import SocketHandler

# 配置日志
logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')
logger = logging.getLogger(__name__)

class Controller:
    def __init__(self, serial_port='/dev/ttyUSB0', baud_rate=38400, 
                 socket_host='0.0.0.0', socket_port=8888):
        """
        初始化控制器
        :param serial_port: 串口设备名称
        :param baud_rate: 波特率
        :param socket_host: Socket监听地址
        :param socket_port: Socket监听端口
        """
        # 创建串口处理器
        self.serial_handler = SerialHandler(
            serial_port=serial_port,
            baud_rate=baud_rate
        )
        
        # 创建Socket处理器
        self.socket_handler = SocketHandler(
            socket_host=socket_host,
            socket_port=socket_port,
            command_callback=self.handle_command
        )
        
        self.running = False
        self.serial_read_thread = None
    
    def handle_command(self, vx, vy, vw):
        """
        处理从Socket接收到的命令
        :param vx: X方向速度
        :param vy: Y方向速度
        :param vw: 角速度
        """
        # 计算线速度并限制在0.7m/s以内
        linear_speed = (vx**2 + vy**2)**0.5  # 计算合成线速度
        
        # 如果线速度超过0.7m/s，等比例缩小vx和vy
        if linear_speed > 0.7:
            scale_factor = 0.7 / linear_speed
            vx = vx * scale_factor
            vy = vy * scale_factor
            logger.info(f"线速度超限，已缩放至0.7m/s: 原始速度={linear_speed:.2f}m/s, 缩放比例={scale_factor:.2f}")
        
        # 将命令发送到串口
        self.serial_handler.send_command(vx, vy, vw)
    
    def read_from_serial(self):
        """
        从串口读取数据并发送到Socket客户端，同时打印日志
        """
        while self.running:
            # 读取串口数据
            data = self.serial_handler.read_data()
            if data:
                # 打印串口返回的数据
                logger.info(f"串口返回: {data}")
                # 发送数据到Socket客户端
                self.socket_handler.send_data(data + '\n')
            
            # 短暂休眠，避免CPU占用过高
            time.sleep(0.01)
    
    def start(self):
        """
        启动控制器
        """
        # 连接串口
        if not self.serial_handler.connect():
            logger.error("无法启动控制器，串口连接失败")
            return False
        
        # 启动Socket处理器
        if not self.socket_handler.start():
            logger.error("无法启动控制器，Socket服务器启动失败")
            self.serial_handler.close()
            return False
        
        # 启动控制器
        self.running = True
        
        # 创建并启动串口读取线程
        self.serial_read_thread = threading.Thread(target=self.read_from_serial)
        self.serial_read_thread.daemon = True
        self.serial_read_thread.start()
        
        logger.info("控制器已启动")
        return True
    
    def stop(self):
        """
        停止控制器
        """
        self.running = False
        
        # 停止Socket处理器
        self.socket_handler.stop()
        
        # 关闭串口连接
        self.serial_handler.close()
        
        logger.info("控制器已停止")

if __name__ == "__main__":
    import argparse
    import platform
    
    # 解析命令行参数
    parser = argparse.ArgumentParser(description='机器人控制器')
    parser.add_argument('--port', type=str, default='/dev/ttyUSB0', help='串口设备名称')
    parser.add_argument('--baud', type=int, default=38400, help='波特率')
    parser.add_argument('--host', type=str, default='0.0.0.0', help='Socket监听地址')
    parser.add_argument('--socket-port', type=int, default=8888, help='Socket监听端口')
    args = parser.parse_args()
    
    # 在Windows系统上，串口名称通常为COM1, COM2等
    if platform.system() == 'Windows':
        default_port = 'COM4'  # 根据实际情况修改
        if args.port == '/dev/ttyUSB0':  # 如果使用了默认值
            args.port = default_port
    
    # 创建并启动控制器
    controller = Controller(
        serial_port=args.port,
        baud_rate=args.baud,
        socket_host=args.host,
        socket_port=args.socket_port
    )
    
    try:
        if controller.start():
            logger.info("按Ctrl+C退出...")
            # 保持主线程运行
            while True:
                time.sleep(1)
    except KeyboardInterrupt:
        logger.info("接收到退出信号")
    finally:
        controller.stop()
